//#ifdef MFC_ENABLED
//
//#include "../app_src/stdafx.h"
//
//#include "xmlnodewrapper.h"
//
//#include "DecoDirectoryManager.h"
//
//#else

#include "stdafx.h"

//#endif



#include "Scene.h"
#include "SceneBuilder.h"
#include "OpenGLRenderInterface.h"

#include "Stat.h"

#include "RenderMiscs.h"

#include <list>


#include "DecoRigidBody.h"
#include "DecoFluidBody.h"
#include "DecoArticulatedBody.h"
#ifdef _SLC
#include "slcSurface.H"
#endif
#include "NavierStokesSolver.h"

#include "DecoLogger.h"
#include "ConfigManager.h"
//#include "densityField.h"
#include "tetVisualizer.h"
#include "ParticleSys.h"
#include "SnapshotManager.h"
#include "SimpleStrokeOptimizer.h"
#include "GregVoxelizerAdapter.h"
#include "VoxelCollisionDetector.h"
//DecoScene* DecoScene::ms_SceneManager = NULL;

//DWORD g_dwTlsDecoScene = TLS_OUT_OF_INDEXES;



//DecoScene* DecoScene::GetSingleton()
//
//{
//
//	if (g_dwTlsDecoScene == TLS_OUT_OF_INDEXES)
//
//	{
//
//		g_dwTlsDecoScene = TlsAlloc();
//
//		TlsSetValue(g_dwTlsDecoScene, NULL);
//
//	}
//
//
//
//	DecoScene* p = (DecoScene*)TlsGetValue(g_dwTlsDecoScene);
//
//	if (p)
//
//		return p;
//
//	else
//
//	{
//
//		p = new DecoScene();
//
//		TlsSetValue(g_dwTlsDecoScene, p);
//
//		return p;
//
//	}
//
//}
//
//void DecoScene::DestroySingleton()
//{
//	if (g_dwTlsDecoScene == TLS_OUT_OF_INDEXES)
//		return;
//	else
//	{
//		DecoScene *pDM = (DecoScene*)TlsGetValue(g_dwTlsDecoScene);
//		if (pDM != NULL)
//		{
//			delete pDM;
//			TlsSetValue(g_dwTlsDecoScene, NULL);
//		}
//	}
//}

tbb::mutex DecoScene::msMutex;


DecoSceneNode::DecoSceneNode(DecoScene* scn, BOOL isLeaf, DecoSceneNode* lchild/* = NULL*/, DecoSceneNode* rchild/* = NULL*/)
: scene(scn)
, leftChild(NULL)
, rightChild(NULL)
, bIsLeaf(isLeaf)

{

	DecoLight* defaultLight = new DecoLight(vector3(0.5f, 0.4f, 0.45f), DecoColor(0x00ffffff), DecoColor(0x00000000), LT_DirectionLight);

	sceneLights.push_back(defaultLight);



	DecoLight* defaultLight1 = new DecoLight(vector3(-0.5f, -0.4f, -0.45f), DecoColor(0x00ffffff), DecoColor(0x00000000), LT_DirectionLight);

	sceneLights.push_back(defaultLight1);

	DecoLight* defaultLight2 = new DecoLight(vector3(-0.5f, 0.4f, -0.45f), DecoColor(0x00ffffff), DecoColor(0x00000000), LT_DirectionLight);

	sceneLights.push_back(defaultLight2);

	DecoLight* defaultLight3 = new DecoLight(vector3(-0.5f, 0.0f, 0.45f), DecoColor(0x00ffffff), DecoColor(0x00000000), LT_DirectionLight);

	sceneLights.push_back(defaultLight3);



	//DecoLight* defaultLight2 = new DecoLight(vector3(0.f, 2.f, 0.f), DecoColor(0x006ec1c2), DecoColor(0xffffffff), LT_PointLight);

	//defaultLight2->SetLightAttenuation(AT_Linear, 0.01f);

	//sceneLights.push_back(defaultLight2);



	//DecoLight* defaultLight2 = new DecoLight(vector3(0.f, 100.f, 0.f), DecoColor(0x000000ff), DecoColor(0x00000000), LT_SpotLight);

	//defaultLight2->SetSpotCutoffAngle(30);

	//defaultLight2->SetSpotDirection(vector3(-1.f, -1.f, 0.f));

	//defaultLight2->SetLightAttenuation(AT_Linear, 0.001f);

	//sceneLights.push_back(defaultLight2);



}

DecoSceneNode::~DecoSceneNode()

{

	//commented out by chenxz

	//leave memory management to DecoObjectFactory ba...



	//if (leftChild)

	//	delete leftChild;

	//if (rightChild)

	//	delete rightChild;

	//if (bIsLeaf)

	//{

	//	for (vector<DecoLight*>::iterator lightIter = sceneLights.begin(); lightIter != sceneLights.end(); lightIter++)

	//	{

	//		if (*lightIter)

	//		{

	//			delete *lightIter ;

	//			*lightIter = NULL;

	//		}

	//	}

	//	for (vector<DecoSceneObject*>::iterator objIter = sceneObjs.begin(); objIter != sceneObjs.end(); objIter++)

	//	{

	//		if (*objIter)

	//		{

	//			DecoObjectFactory::Destroy( *objIter );

	//			*objIter = NULL;

	//		}

	//	}

	//}

	//vector<DecoSceneObject*> sceneObjs;

	//vector<DecoLight*> sceneLights;

}



void DecoSceneNode::Render(DecoRenderInterface* RI, DecoCamera* camera, SceneObjectType objType, DecoDrawType drawType)

{

	if (bIsLeaf)
	{
		//RI->Clear(0, DecoColor(0x00000000), 1);



		RenderAllSceneObjects(RI, camera, objType, drawType);


		if (objType == AllT)
		{
			DecoSceneObject* focusObj = scene->GetFocusObject();

			if(focusObj != NULL)

			{

				RI->SetColor(0xffffffff);

				assert((focusObj)->GetRenderData());

				Box box = (focusObj)->GetRenderData()->GetBoundingBox();

				DecoRenderMisc::GetSingleton()->DrawBox(NULL, box);

			}
		}

	} 

	else

	{

		if (leftChild)

			leftChild->Render(RI, camera, objType, drawType);

		if (rightChild)

			rightChild->Render(RI, camera, objType, drawType);

	}

}



void DecoSceneNode::RenderAllSceneObjects(DecoRenderInterface* RI, DecoCamera* camera, SceneObjectType objType, DecoDrawType drawType)

{

	vector<DecoRenderData*> tempSortList;

	vector<DecoRenderData*> translucentDrawList;

	vector<DOUBLE> distanceList;

	ConvexVolume viewFrustum = camera->getViewFrustum();

	INT toRender = 0, beRendered = 0;

	DecoRenderData* floor = NULL, *ceiling = NULL;

	DecoLight* Lights[MAX_LIGHT];

	INT numEffectiveLights = 0;

	for (vector<DecoSceneObject*>::iterator objIter = sceneObjs.begin(); objIter != sceneObjs.end(); objIter++)

	{

		DecoSceneObject* obj = *objIter;

		if (obj->IsRenderable() && (objType == AllT || obj->GetType() == objType))

		{

			DecoRenderData* objDecoRenderData = obj->GetRenderData();

			assert(objDecoRenderData);

			//if (viewFrustum.BoxCheck(objDecoRenderData->GetBoundingBox()) != CF_Outside)

			{
				if (objDecoRenderData->NeedSort())

				{
					DOUBLE distSqr = (camera->getEye() - objDecoRenderData->GetBoundingBox().GetCenter()).lengthSqr();

					distanceList.push_back(distSqr);

					sort(distanceList.begin(), distanceList.end());

					vector<DOUBLE>::iterator it = find(distanceList.begin(), distanceList.end(), distSqr);

					translucentDrawList.insert(translucentDrawList.begin() + (it - distanceList.begin()), objDecoRenderData);

				}

				else

				{

					GetRelevantLights(objDecoRenderData, numEffectiveLights, Lights);

					objDecoRenderData->Render(RI, Lights, numEffectiveLights, drawType);

					DecoStat::GetSingleton()->CumulateObjectRendered(1);

				}



#ifdef DEBUG_BOUNDINGBOX

				RI->SetColor(0xff0000ff);

				objDecoRenderData->RenderBoundingBox(RI);

#endif
			}

			DecoStat::GetSingleton()->CumulateObjectTotal(1);

		}

	}


	for (vector<DecoRenderData*>::iterator translucentRenderDataIter = translucentDrawList.begin(); translucentRenderDataIter != translucentDrawList.end(); translucentRenderDataIter++)

	{

		GetRelevantLights((*translucentRenderDataIter), numEffectiveLights, Lights);

		(*translucentRenderDataIter)->RenderOpaqueSection(RI, Lights, numEffectiveLights, drawType);

		DecoStat::GetSingleton()->CumulateObjectRendered(1);

	}

	for (vector<DecoRenderData*>::iterator translucentRenderDataIter = translucentDrawList.begin(); translucentRenderDataIter != translucentDrawList.end(); translucentRenderDataIter++)

	{

		GetRelevantLights((*translucentRenderDataIter), numEffectiveLights, Lights);

		(*translucentRenderDataIter)->RenderTransparentSection(RI, Lights, numEffectiveLights, drawType);

	}

}



void DecoSceneNode::GetRelevantLights(const DecoRenderData* obj, INT& numLights, DecoLight** lights)

{

	numLights = 0;

	for (vector<DecoLight*>::iterator lightIter = sceneLights.begin(); lightIter != sceneLights.end(); lightIter++)

	{

		assert(numLights < MAX_LIGHT);

		lights[numLights] = *(lightIter);

		numLights++;

	}
}



DecoScene::DecoScene() : numXElement(0), numYElement(0), numZElement(0), stepSize(0.0), tmpVoxelizerBufferX(NULL), solidElements(NULL), buffer(NULL), numXElementVoxelize(0), numYElementVoxelize(0), numZElementVoxelize(0), time(0), timeStep(0.03)
{
	tbb::mutex::scoped_lock lock(msMutex);
	RI = new DecoOpenGLRenderInterface();
#ifdef WIN32 //jietan hack weird opengl does not work on Karen's machine
	RI->Initialize();
#endif 
	camera = new DecoCamera(RI);
	DecoRenderMisc::Initialize(RI);
	//only one node currently
	rootSceneNode = new DecoSceneNode(this, TRUE, NULL, NULL);
	DecoConfig::GetSingleton()->GetDouble("Scene", "timeStep", timeStep);
	BOOL bSimplifiedFluid = TRUE;
	DecoConfig::GetSingleton()->GetBool("Scene", "bSimplifiedFluid", bSimplifiedFluid);
	if (bSimplifiedFluid)
		timeStep = 0.01;
	solver.SetScene(this);
	particles.SetScene(this);
	particles.SetSolver(&solver);
}

void DecoScene::DumpObjFiles(const std::string& fileName)
{
	vector <DecoSceneObject*> allObjs;
	GetAllSceneObjects(allObjs);
	for (vector<DecoSceneObject*>::iterator it = allObjs.begin(); it != allObjs.end(); ++it)
	{
		(*it)->GetRenderData()->DumpToObjFile(fileName);
	}
}

void DecoScene::SetGridsInformation()
{
	solver.solidElements = solidElements;
	solver.SetGridField( &grids );

#ifdef _PARTICLES
	particles.SetGridField( &grids );
#endif

#ifdef _DENSITY
	DensityField::GetSingleton()->SetGridField( &grids );
#endif

#ifdef _SLC
	SLC::GetSingleton()->SetGridField( &grids );
#endif

}


void DecoScene::SetNumXElement (UINT num)
{
	INT bUpdateGridType = 1;
	DecoConfig::GetSingleton()->GetInt("Solver", "bUpdateGridType", bUpdateGridType);
	assert(num);
	vector3 ext = scnBox.GetExtent();
	assert(ext.lengthSqr() > EPSILON_FLOAT);
	//num = round2Pow2(num);
	numXElement = num;
	stepSize = 2 * ext.x / numXElement;
	numYElement = static_cast<UINT>(ceil(2 * ext.y / stepSize));
	numZElement = static_cast<UINT>(ceil(2 * ext.z / stepSize));
	if (bUpdateGridType)
	{
		numYElement = round2Pow2(numYElement);
		numZElement = round2Pow2(numZElement);
	}
	assert(numYElement);
	assert(numZElement);
	scnBox = scnBox.ExpandBy(vector3(0, numYElement * stepSize / 2 - ext.y, numZElement * stepSize / 2 - ext.z)); //scnbox will be adjust to make stepSize in x, y, z equal
	grids.SetSceneBox(scnBox);

	grids.SetNumElement(numXElement, numYElement, numZElement);

}


void DecoScene::AllocateTempBuffer()
{
	if (tmpVoxelizerBufferX)
		delete[] tmpVoxelizerBufferX;

	INT xRes = 0, yRes = 0, zRes = 0;
	int iVoxelPrecsion = 3;
#ifdef _DENSITY
	DensityField::GetSingleton()->GetEffectResolution(xRes, yRes, zRes);
#endif
	//DecoConfig::GetSingleton()->GetInt("Resolution", "xElementsVoxel", numXElementVoxelize);
	numXElementVoxelize = numXElement * iVoxelPrecsion;
	DOUBLE voxStep = scnBox.GetExtent().x * 2 / numXElementVoxelize;
	numYElementVoxelize = static_cast<INT>(scnBox.GetExtent().y * 2 / voxStep + 0.5);
	numZElementVoxelize = static_cast<INT>(scnBox.GetExtent().z * 2 / voxStep + 0.5);
	if (xRes > numXElementVoxelize)
	{
		if (solidElements)
			Free3DArray<BYTE>(solidElements, xRes, yRes, zRes);
		tmpVoxelizerBufferX = new DWORD[zRes * yRes];
		solidElements = Malloc3DArray<BYTE>(xRes, yRes, zRes, TRUE);
	}
	else
	{
		if (solidElements)
			Free3DArray<BYTE>(solidElements, numXElementVoxelize, numYElementVoxelize, numZElementVoxelize);
		tmpVoxelizerBufferX = new DWORD[numZElementVoxelize * numYElementVoxelize];
		solidElements = Malloc3DArray<BYTE>(numXElementVoxelize, numYElementVoxelize, numZElementVoxelize);
	}
}

//#define _GPU_VOXELIZER

void DecoScene::sliceRender(DecoRenderData* obj, BYTE*** solidElements, INT numSlices, INT height, INT width, INT id)
{
	DecoTimer timer;
	timer.startTimer();
	//Zero3DArray(solidElements, numXElementVoxelize, numYElementVoxelize, numZElementVoxelize);
#ifdef _GPU_VOXELIZER
	RI->PushState();
	DecoCamera tmpCamera = *camera;

	RI->SetViewport(0, 0, width, height);
	FLOAT beginX = static_cast<FLOAT>(scnBox[0].x);
	FLOAT sliceBreadth = static_cast<FLOAT>(scnBox.GetExtent().x * 2 / numSlices);
	FLOAT camWidth = static_cast<FLOAT>(scnBox.GetExtent().z * 2);
	FLOAT camHeight = static_cast<FLOAT>(scnBox.GetExtent().y * 2);
	FLOAT camPosX = beginX - 0.5 * sliceBreadth;
	vector3 verts[6];
	verts[0] = vector3(10, -camHeight / 2, -camWidth / 2);
	verts[1] = vector3(10, camHeight / 2, -camWidth / 2);
	verts[2] = vector3(10, camHeight / 2, camWidth / 2);
	verts[3] = vector3(10, -camHeight / 2, -camWidth / 2);
	verts[4] = vector3(10, camHeight / 2, camWidth / 2);
	verts[5] = vector3(10, -camHeight / 2, camWidth / 2);
	camera->set(vector3(camPosX, scnBox.GetCenter().y, scnBox.GetCenter().z), vector3(beginX, scnBox.GetCenter().y, scnBox.GetCenter().z), vector3(0, 1, 0));

	Box bbox = obj->GetBoundingBox();
	INT startSlice = (bbox.GetMin().x - camPosX) / sliceBreadth + 1;
	startSlice = max(1, startSlice);
	INT endSlice = (bbox.GetMax().x - camPosX) / sliceBreadth;
	endSlice = min(numSlices, endSlice);
	
	RI->EnableDepthTest(FALSE);
	glEnable(GL_BLEND);
	glBlendFunc(GL_ONE, GL_ONE);

	INT iBit = 0;
	INT iStartCopySlice = startSlice;
	RI->Clear(TRUE, DecoColor(0x0));

	for (INT ithSlice = startSlice; ithSlice <= endSlice; ithSlice++)
	{
		//static INT counter = 1000;
		char filename[100];
		FLOAT nearDist = ithSlice * sliceBreadth;
		camera->setOrtho(camWidth, camHeight, nearDist, DEFAULT_FAR_PLANE);

		RI->Clear(FALSE, DecoColor(0x0));
		RI->SetColor(DecoColor(1 << iBit));
		iBit++;

		RI->SetStencilState(SS_Accumulate);
		RI->SetCullMode(CM_CCW);
		obj->Render(RI, NULL, 0);//, DT_FrontPolygon);

		//RI->EnablePxLogicOp(LO_Xor);
		RI->SetStencilState(SS_Deduct);
		RI->SetCullMode(CM_CW);
		obj->Render(RI, NULL, 0);//, DT_BackPolygon);

		RI->SetStencilState(SS_Test);
		DecoVertexBuffer tmpVB;
		tmpVB.SetVertexInformation(6, verts);
		RI->SetCullMode(CM_None);
		RI->DrawPrimitive(PT_TriangleList, 2, DT_SolidPolygon, &tmpVB);
// 		sprintf(filename, "../tmpPic/slice%d.jpg", ithSlice);
// 		RI->SaveToImage(filename);
		assert(tmpVoxelizerBufferX);


		if(iBit == 24 || ithSlice == endSlice)
		{
			RI->ReadFrameBuffer(BT_Back, CC_RGBA, tmpVoxelizerBufferX);
			for(UINT i = 0; i < iBit; i++, iStartCopySlice++)
			{
				UINT iMask = 1 << i;
				for (INT j = 0; j < height; j++)
				{
					for (INT k = 0; k < width; k++)
					{
						if ((tmpVoxelizerBufferX[j * width + k] & iMask) != 0)
						{
							solidElements[iStartCopySlice - 1][j][k] = 1;
							solidElementsStorage[iStartCopySlice - 1][j][k] = 1;
						}
					}
				}
			}

			RI->Clear(TRUE, DecoColor(0x0));
			iBit = 0;
		}
	}

	glDisable(GL_BLEND);
	RI->SetStencilState(SS_None);

	*camera = tmpCamera;
	RI->PopState();
#else
#if 1

	FLOAT sliceBreadth = static_cast<FLOAT>(scnBox.GetExtent().x * 2 / numSlices);
	Box bbox = obj->GetBoundingBox();
	INT startSlice = (bbox.GetMin().x - scnBox.GetMin().x) / sliceBreadth;
	//startSlice = max(1, startSlice);
	INT endSlice = (bbox.GetMax().x - scnBox.GetMin().x) / sliceBreadth;
	//endSlice = min(numSlices, endSlice);
	INT startHeight = (bbox.GetMin().y - scnBox.GetMin().y) / sliceBreadth;
	//startHeight = max(1, startHeight);
	INT endHeight = (bbox.GetMax().y - scnBox.GetMin().y) / sliceBreadth;
	//endHeight = min(height, endHeight);
	INT startWidth = (bbox.GetMin().z - scnBox.GetMin().z) / sliceBreadth;
	//startWidth = max(1, startWidth);
	INT endWidth = (bbox.GetMax().z - scnBox.GetMin().z) / sliceBreadth;
	//endWidth = min(height, endWidth);

	const BYTE mask = 1 << id;

	for (int i = startSlice; i < endSlice; ++i)
	{
		for (int j = startHeight; j < endHeight; ++j)
		{
			for (int k = startWidth; k < endWidth; ++k)
			{
				vector3 cellCenter = grids.GetSubGridCenter(i, j, k);
				if (obj->IsPointInside(cellCenter, true))
				{
					solidElements[i][j][k] |= mask;
					//solidElementsStorage[i][j][k] = 1;
				}
			}
		}
	}

#else
	GregVoxelizerAdapter vox;
	vox.Voxelize(obj, scnBox, solidElements, numSlices, height, width);
#endif
#endif
	double time = timer.getCurrentTime();
	//(*DecoLogger::GetSingleton()) << "Voxelization spent " << time << " seconds.\n";
}

void DecoScene::UpdateSolidTransformation(bool bToTarget)
{
	vector<DecoSceneObject*>& sceneObjs = rootSceneNode->GetSceneObjects();
	for(vector<DecoSceneObject*>::iterator it = sceneObjs.begin(); it != sceneObjs.end(); ++it)
	{
		DecoSceneObject& obj = (**it);
		if (obj.GetType() != ArticulatedT)
			continue;

		DecoArticulatedObject* articulatedObj = static_cast<DecoArticulatedObject*>(&obj);
		articulatedObj->UpdateTransformation();

		if(bToTarget)
			articulatedObj->UpdateToTarget();
	}
}

void DecoScene::UpdateSolidBeforeCoupling()
{
	vector<DecoSceneObject*>& sceneObjs = rootSceneNode->GetSceneObjects();

	for(vector<DecoSceneObject*>::iterator it = sceneObjs.begin(); it != sceneObjs.end(); ++it)
	{
		DecoSceneObject& obj = (**it);
		if (obj.GetType() != SolidT && obj.GetType() != ArticulatedT)
		{
			continue;
		}
		DecoSolidObject* solidObj = static_cast<DecoSolidObject*>(&obj);
		solidObj->UpdateBeforeCoupling(timeStep, FALSE);
	}
}

void DecoScene::UpdateSolid()
{
	vector<DecoSceneObject*>& sceneObjs = rootSceneNode->GetSceneObjects();

	for(vector<DecoSceneObject*>::iterator it = sceneObjs.begin(); it != sceneObjs.end(); ++it)
	{
		DecoSceneObject& obj = (**it);
		if (obj.GetType() != SolidT && obj.GetType() != ArticulatedT)
		{
			continue;
		}
		obj.Update(timeStep);
	}
}


inline vector3 cross(const vector3 &a, const vector3 &b)
{
	vector3 result;

	result[0] = a[1] * b[2] - a[2] * b[1];	
	result[1] = a[2] * b[0] - a[0] * b[2];	
	result[2] = a[0] * b[1] - a[1] * b[0];	

	return result;
}

void DecoScene::CalcForces()
{
	vector<DecoSceneObject*>& sceneObjs = rootSceneNode->GetSceneObjects();
	for(vector<DecoSceneObject*>::iterator it = sceneObjs.begin(); it != sceneObjs.end(); ++it)
	{
		DecoSceneObject& obj = (**it);
		DecoArticulatedObject* articulatedObj = static_cast<DecoArticulatedObject*>(&obj);

		float fA = GetSpaceStepSize() * GetSpaceStepSize();
		int numBodies = articulatedObj->GetNumBodies();

		mvCOMs[numBodies] = vector3(0,0,0);
		int iGridCount = 0;
		for (int i = 0; i < numBodies; ++i)
		{
			mvBodyForces[i] = vector3(0,0,0);
			mvCOMs[i] = vector3(0,0,0);
			for(int j = 0; j < articulatedObj->mGridIndicesForBodies[i].size(); j++)
			{
				Index3 idx = articulatedObj->mGridIndicesForBodies[i][j];
				vector3 vCurCOM = grids.GetGridCenter(idx.m_i, idx.m_j, idx.m_k);
				mvCOMs[i] += vCurCOM;
				mvCOMs[numBodies] += vCurCOM;
				iGridCount++;

				if(grids.isSolidCell(idx.m_i, idx.m_j, idx.m_k) && grids.isFluidCell(idx.m_i - 1, idx.m_j, idx.m_k))
				{
					mvBodyForces[i] += grids.p[idx.m_i - 1][idx.m_j][idx.m_k] * fA * vector3(1,0,0);
				}
				if(grids.isSolidCell(idx.m_i, idx.m_j, idx.m_k) && grids.isFluidCell(idx.m_i + 1, idx.m_j, idx.m_k))
				{
					mvBodyForces[i] += grids.p[idx.m_i + 1][idx.m_j][idx.m_k] * fA * vector3(-1,0,0);
				}
				if(grids.isSolidCell(idx.m_i, idx.m_j, idx.m_k) && grids.isFluidCell(idx.m_i, idx.m_j - 1, idx.m_k))
				{
					mvBodyForces[i] += grids.p[idx.m_i][idx.m_j - 1][idx.m_k] * fA * vector3(0,1,0);
				}
				if(grids.isSolidCell(idx.m_i, idx.m_j, idx.m_k) && grids.isFluidCell(idx.m_i, idx.m_j + 1, idx.m_k))
				{
					mvBodyForces[i] += grids.p[idx.m_i][idx.m_j + 1][idx.m_k] * fA * vector3(0,-1,0);
				}
				if(grids.isSolidCell(idx.m_i, idx.m_j, idx.m_k) && grids.isFluidCell(idx.m_i, idx.m_j, idx.m_k - 1))
				{
					mvBodyForces[i] += grids.p[idx.m_i][idx.m_j][idx.m_k - 1] * fA * vector3(0,0,1);
				}
				if(grids.isSolidCell(idx.m_i, idx.m_j, idx.m_k) && grids.isFluidCell(idx.m_i, idx.m_j, idx.m_k + 1))
				{
					mvBodyForces[i] += grids.p[idx.m_i][idx.m_j][idx.m_k + 1] * fA * vector3(0,0,-1);
				}
			}

			mvCOMs[i] /= articulatedObj->mGridIndicesForBodies[i].size();
		}
		mvCOMs[numBodies] /= iGridCount;

		mvTorque = vector3(0,0,0);
		for (int i = 0; i < numBodies; ++i)
		{
			mvTorque += cross(mvBodyForces[i], mvCOMs[i] - mvCOMs[numBodies]);
		}
	}

}

void DecoScene::VoxelizeSolid()
{
	vector<DecoSceneObject*>& sceneObjs = rootSceneNode->GetSceneObjects();
	CopyTo3DArray(grids.prevInDomain, grids.inDomain, grids.numXElement, grids.numYElement, grids.numZElement);
	grids.ClearTypeFlagTo(GT_Air);
	MemSet3DArray(grids.uFluidVol, 1.f, grids.numXElement + 1, grids.numYElement, grids.numZElement);
	MemSet3DArray(grids.vFluidVol, 1.f, grids.numXElement, grids.numYElement + 1, grids.numZElement);
	MemSet3DArray(grids.wFluidVol, 1.f, grids.numXElement, grids.numYElement, grids.numZElement + 1);
	MemSet3DArray(grids.cFluidVol, 1.f, grids.numXElement, grids.numYElement, grids.numZElement);

	INT bUseSceneBuilderFile = FALSE;
	DecoConfig::GetSingleton()->GetInt("Scene", "useSceneBuilder", bUseSceneBuilderFile);
	if (bUseSceneBuilderFile)
		SceneBuilder::GetSingleton()->EnforceSolidSceneWhenUpdate(this);

	//Zero3DArray(solidElementsStorage, numXElementVoxelize, numYElementVoxelize, numZElementVoxelize);
	Zero3DArray(solidElements, numXElementVoxelize, numYElementVoxelize, numZElementVoxelize);

	for(vector<DecoSceneObject*>::iterator it = sceneObjs.begin(); it != sceneObjs.end(); ++it)
	{
		DecoSceneObject& obj = (**it);
		if (obj.GetType() != SolidT && obj.GetType() != ArticulatedT)
		{
			continue;
		}
		GridField* grid = &grids;

		INT width = numZElementVoxelize;
		INT height = numYElementVoxelize;
		INT numSlices = numXElementVoxelize;
		obj.SetGrid(&grids);

		if (obj.GetType() == ArticulatedT)
		{
			DecoArticulatedObject* articulatedObj = static_cast<DecoArticulatedObject*>(&obj);
			articulatedObj->ResetAssociateGridIndex();
			articulatedObj->mGridIndicesForBodies.clear();
			
			//articulatedObj->UpdateTransformation();
			int numBodies = articulatedObj->GetNumBodies();
			for (int i = 0; i < numBodies; ++i)
			{
				sliceRender(articulatedObj->GetRenderData(i), solidElements, numSlices, height, width, i);
				const vector<Index3>& associateIndices = grids.updateSolidType(obj, solidElements, numSlices, height, width, i);
				obj.AddAssociateGridIndex(associateIndices);
				articulatedObj->mGridIndicesForBodies.push_back(associateIndices);
				grids.updateSolidVelocity(obj);
			}

			articulatedObj->calculateMass();
			articulatedObj->calculateInertia();

		}
		else if (obj.GetType() == SolidT)
		{
			sliceRender(obj.GetRenderData(), solidElements, numSlices, height, width, 0);
			const vector<Index3>& associateIndices = grids.updateSolidType(obj, solidElements, numSlices, height, width, 0);
			obj.SetAssociateGridIndex(associateIndices);
			grids.updateSolidVelocity(obj);
			DecoRigidObject* rigidObj = static_cast<DecoRigidObject*>(&obj);

			rigidObj->calculateMass();
			rigidObj->calculateInertia();


		}

	}
}

void DecoScene::VoxelizeSprayer()
{
	INT width = numZElementVoxelize;
	INT height = numYElementVoxelize;
	INT numSlices = numXElementVoxelize;
	vector<DecoSceneObject*>& sceneObjs = rootSceneNode->GetSceneObjects();

	for(vector<DecoSceneObject*>::iterator it = sceneObjs.begin(); it != sceneObjs.end(); ++it)
	{
		if ((**it).GetType() == SprayerT)
		{
#if defined _PARTICLES && !defined _DENSITY
			sliceRender((**it).GetRenderData(), solidElements, numSlices, height, width);
			particles.FillParticles(solidElements, numSlices, height, width);
#elif defined _DENSITY
			INT xRes, yRes, zRes;
			DensityField::GetSingleton()->GetEffectResolution(xRes, yRes, zRes);
			sliceRender((**it), solidElements, xRes, yRes, zRes);
			DensityField::GetSingleton()->FillSmoke(solidElements, xRes, yRes, zRes);
#endif

			grids.VoxelizeSprayer(solidElements, numSlices, height, width);

		}
	}
}



void DecoScene::VoxelizeFluid()
{
	INT width = numZElementVoxelize;
	INT height = numYElementVoxelize;
	INT numSlices = numXElementVoxelize;

	vector<DecoSceneObject*>& sceneObjs = rootSceneNode->GetSceneObjects();

	for(vector<DecoSceneObject*>::iterator it = sceneObjs.begin(); it != sceneObjs.end(); ++it)
	{
		if ((**it).GetType() == FluidT)
		{
#ifdef _SLC
			SLC::GetSingleton()->InitializeFromStaticMesh(static_cast<DecoStaticMesh*>((**it).GetRenderData()));
#elif defined _PARTICLES && !defined _DENSITY
			sliceRender((**it).GetRenderData(), solidElements, numSlices, height, width);
			particles.FillParticles(solidElements, numSlices, height, width);
#elif defined _DENSITY
			INT xRes, yRes, zRes;
			DensityField::GetSingleton()->GetEffectResolution(xRes, yRes, zRes);
			sliceRender((**it), solidElements, xRes, yRes, zRes);
			DensityField::GetSingleton()->FillSmoke(solidElements, xRes, yRes, zRes);
#endif
		}
	}
#ifdef _SLC
	SLC::GetSingleton()->ConstructAccurateFluidSurface();
#endif
}


void DecoScene::Voxelize()
{
	vector<DecoSceneObject*>& sceneObjs = rootSceneNode->GetSceneObjects();

	VoxelizeSolid();
	//VoxelizeSprayer();
	VoxelizeFluid();


}

void DecoScene::PointScreen2WorldRay(const vector2& pt, vector3& outOrigin, vector3& outDir) const 

{

	vector3 viewVec = PointScreen2Camera(pt);

	matrix44 invview;

	RI->GetTransform(TT_WorldToCamera, invview);

	invview.invert();



	if (camera->isOrthogonal())

	{

		vector4 viewOrigin(viewVec);

		viewOrigin.z = 0.f;

		vector4 viewDir(0, 0, -1, 0);

		viewOrigin.w = 1.f;

		viewOrigin = invview * vector4(viewOrigin);

		vector4 worldVec = invview * viewDir;

		outOrigin = vector3(viewOrigin.x, viewOrigin.y, viewOrigin.z);

		outDir = vector3(worldVec.x, worldVec.y, worldVec.z);

	}

	else

	{

		viewVec.normalize();

		outOrigin = camera->getEye();	

		vector4 worldVec = invview * vector4(viewVec);

		outDir = vector3(worldVec.x, worldVec.y, worldVec.z);

	}
}

void DecoScene::ToggleShowGrid()
{
	solver.ToggleShowGridLayer();
}

void DecoScene::RenderGridField()
{
	DecoLight* Lights[MAX_LIGHT];
	INT numEffectiveLights = 0;
	rootSceneNode->GetRelevantLights(NULL, numEffectiveLights, Lights);

	solver.Render(RI, Lights, numEffectiveLights);

}


DecoColor Shading(const vector3& realIntersectPt, const vector3& norm, const vector3& lightPos, const vector3& viewPos,
				  const vector3& lightDiffuse, const vector3& lightSpecular, const vector3& lightAmbient, 
				  const vector3& matDiffuse, const vector3& matSpecular, const vector3&matAmbient)
{
	vector3 col(0, 0, 0);
	vector3 rayDir(lightPos - realIntersectPt);
	vector3 viewDir(viewPos - realIntersectPt);
	vector3 normal = norm;
	viewDir.normalize();
	rayDir.normalize();
	normal.normalize();
	vector3 halfDir = (viewDir + rayDir) / 2;
	halfDir.normalize();

	col += vector3(lightDiffuse.x * matDiffuse.x, lightDiffuse.y * matDiffuse.y, lightDiffuse.z * matDiffuse.z) * max(0.0, DotProduct(normal, rayDir));
	//col += vector3(lightAmbient.x * matAmbient.x, lightAmbient.y * matAmbient.y, lightAmbient.z * matAmbient.z);
	DOUBLE dotHalfAngle = max(0.0, DotProduct(halfDir, normal));
	col += vector3(lightSpecular.x * matSpecular.x, lightSpecular.y * matSpecular.y, lightSpecular.z * matSpecular.z) * dotHalfAngle * dotHalfAngle;
	return DecoColor(vector4(col.x, col.y, col.z, 1));
}

DecoColor Shading(const vector3& realIntersectPt, const vector3& norm, DecoLight** lights, INT numLights, const vector3& viewPos, DecoMaterial* mat)
{
	DecoColor col(0, 0, 0);
	vector3 matDiffuse, matSpecular, matAmbient;
	vector3 lightPos, lightDiffuse, lightSpecular, lightAmbient;
	if (mat)
	{
		matDiffuse = mat->GetDiffuse();
		matSpecular = mat->GetSpecular();
		matAmbient = mat->GetAmbient();
	}
	else
	{
		matDiffuse = vector3(1, 1, 1);
		matSpecular = vector3(0, 0, 0);
		matAmbient = vector3(0.5, 0.5, 0.5);
	}
	for (INT i = 0; i < numLights; i++)
	{
		lightPos = lights[i]->GetPosition();
		lightDiffuse = lights[i]->GetDiffuse();
		lightSpecular = lights[i]->GetSpecular();
		col += Shading(realIntersectPt, norm, lightPos,  viewPos, lightDiffuse, lightSpecular, lightAmbient, matDiffuse, matSpecular, matAmbient);
	}		
	lightAmbient = vector3(0.5, 0.5, 0.5);
	col += DecoColor(vector4(lightAmbient.x * matAmbient.x, lightAmbient.y * matAmbient.y, lightAmbient.z * matAmbient.z, 1));

	return col;
}

#ifdef _DENSITY
void DecoScene::RenderVolume(BOOL bViewModel)
{
	//RI->Clear(1, DecoColor(0x000000ff), 1);
	DecoColor bkColor(0xff000000);
	DecoColor rhoColor(0xffffffff); 
	INT width = 0, height = 0, startX = 0, startY = 0;
	RI->GetViewport(startX, startY, width, height);
	INT numPx = width * height;
	if (!buffer)
		buffer = new DecoColor[numPx];
	INT ithPx = 0;
	vector3 viewPos = camera->getEye();
	INT xRes = 0, yRes = 0, zRes = 0;
	DensityField::GetSingleton()->GetEffectResolution(xRes, yRes, zRes);
	DOUBLE stepSize = scnBox.GetExtent().x * 2 / (xRes - 1);
	DecoLight* Lights[MAX_LIGHT];
	INT numEffectiveLights = 0;
	rootSceneNode->GetRelevantLights(NULL, numEffectiveLights, Lights);
#ifdef _PARTICLES
	DensityField::GetSingleton()->ConvertFromParticles();
#endif
	for (INT i = 0; i < height; i++)
	{
		for (INT j = 0; j < width; j++)
		{
			buffer[ithPx] = bkColor;
			DOUBLE maxRayLength = DEFAULT_FAR_PLANE;

			vector2 pt(j, i);
			vector3 origin, dir;
			PointScreen2WorldRay(pt, origin, dir);
			if (bViewModel)
			{
				vector<DecoSceneObject* > & sceneObjs =  rootSceneNode->GetSceneObjects();
				INT numObjs = static_cast<INT>(sceneObjs.size());
				for (INT it = 0; it < numObjs; it++)
				{
					DecoSceneObject* obj = sceneObjs[it];
					if (obj->GetType() != SolidT || !obj->IsRenderable())
						continue;
					DOUBLE time;
					vector3 intersectionPt, norm;
					if (obj->GetRenderData()->GetBoundingBox().RayCheck(origin, dir) <= 0)
						continue;
					if (obj->GetRenderData()->RayIntersection(origin, dir, intersectionPt, time, &norm))
					{
						DOUBLE distFromEye = (intersectionPt - origin).length();
						if (distFromEye < maxRayLength)
						{
							DecoStaticMesh* mesh = static_cast<DecoStaticMesh*>(obj->GetRenderData());
							buffer[ithPx] = Shading(intersectionPt, norm, Lights, numEffectiveLights, viewPos, mesh->GetFirstValidMaterial());
							maxRayLength = distFromEye;
						}
					}
				}
			}
			//Box bb = DensityField::GetSingleton()->GetSmokeBoundingBox();
			//vector3 center = bb.GetCenter();
			//DOUBLE radiusSqr = bb.GetExtent().lengthSqr();
			//vector3 projPoint = origin + DotProduct(center - origin, dir) / DotProduct(dir, dir) * dir;	
			//DOUBLE distSqr = (center - origin).lengthSqr() - (projPoint - origin).lengthSqr();

			//if (distSqr >= radiusSqr)
			//{
			//	buffer[ithPx] = bkColor;
			//	ithPx++;
			//	continue;
			//}
			DOUBLE timeIntersection = scnBox.RayCheck(origin, dir);
			DOUBLE rayLength = ((timeIntersection > 0) ? (timeIntersection * dir.length()) : 0);
			vector3 realIntersectPt;
			DOUBLE rhoAccumulator = 0;
			DOUBLE rho = 0;
			while (TRUE)
			{
				if (rayLength >= maxRayLength)
					break;

				vector3 worldPt = origin + rayLength * dir;
				rho = DensityField::GetSingleton()->SampleRho(worldPt);
				assert(rho >= 0 && rho <= 1);
				rhoAccumulator += rho * stepSize;
				rayLength += stepSize;

			}
			rhoAccumulator = Clamp(rhoAccumulator, 0.0, 1.0);
			buffer[ithPx] = rhoAccumulator * rhoColor + (1 - rhoAccumulator) * buffer[ithPx];

			ithPx++;
		}
	}
	//image* saveImage = new image(width, height);
	//static INT counter = 1000;
	//char filename[100];
	//sprintf(filename, "tmpPic\\%d.jpg", counter++);
	//saveImage->ReadFromBuffer(buffer, 4);
	//BOOL result = saveImage->SaveToFile(filename);
	//delete saveImage;
	DecoCamera tmpCamera = *camera;
	RI->PushState(RS_ViewMatrix);
	RI->PushState(RS_ProjectionMatrix);
	camera->set(vector3(0, 0, 1), vector3(0, 0, 0), vector3(0, 1, 0));
	camera->setOrtho(static_cast<FLOAT>(width), static_cast<FLOAT>(height), static_cast<FLOAT>(DEFAULT_NEAR_PLANE), DEFAULT_FAR_PLANE);
	RI->WriteFrameBuffer(-width / 2, -height / 2, width, height, BT_Back, CC_RGBA, buffer);
	*camera = tmpCamera;
	RI->PopState();
	RI->PopState();
}
#endif

vector3 DecoScene::vectorScreen2World(const vector2& start, const vector2& end, DOUBLE startDistance, Plane constraint[], INT numConstraints) const

{

	if (!numConstraints)

	{

		vector3 viewVecStart = PointScreen2Camera(start);

		vector3 viewVecEnd = PointScreen2Camera(end);

		vector3 viewVec = viewVecEnd - viewVecStart;

		matrix44 invview;

		RI->GetTransform(TT_WorldToCamera, invview);

		invview.invert();

		vector4 worldVec = invview * vector4(viewVec);



		if (!camera->isOrthogonal())

		{

			DOUBLE nearD = camera->getNearDistance();

			DOUBLE nearDistance = sqrt(viewVec.lengthSqr() + nearD * nearD);

			if (startDistance == 0)

				startDistance = nearDistance;

			DOUBLE lengthRatio = startDistance / nearDistance;



			worldVec *= lengthRatio;

		}

		return vector3(worldVec.x, worldVec.y, worldVec.z);

	}

	else

	{

		assert(numConstraints == 1);

		vector3 originStart, originEnd, dirStart, dirEnd;

		PointScreen2WorldRay(start, originStart, dirStart);

		PointScreen2WorldRay(end, originEnd, dirEnd);

		vector3 intersectStart = constraint[0].RayIntersect(originStart, dirStart);

		vector3 intersectEnd = constraint[0].RayIntersect(originEnd, dirEnd);

		return (intersectEnd - intersectStart);

	}

}



vector3 DecoScene::PointScreen2Camera(const vector2& pt) const

{

	INT x, y, wid, ht;



	RI->GetViewport(x, y, wid, ht);

	vector2 canonicalPt(pt.x - (x + wid / 2), pt.y - (y + ht / 2));

	canonicalPt.x /= (wid / 2);

	canonicalPt.y /= (ht / 2);

	vector3 viewVec;

	FLOAT nearD = camera->getNearDistance();



	if (camera->isOrthogonal())

	{

		FLOAT width = camera->getWidth();

		FLOAT height = camera->getHeight();

		viewVec = vector3(canonicalPt.x * width / 2, canonicalPt.y * height / 2, -nearD);

	}

	else

	{

		FLOAT vAng = camera->getVerticalViewAngle();

		FLOAT aspRatio = static_cast<FLOAT>(wid) / ht;

		DOUBLE halfHeight = nearD * tan(vAng * PI / 360.f);

		DOUBLE halfWidth = aspRatio * halfHeight;



		viewVec = vector3(canonicalPt.x * halfWidth, canonicalPt.y * halfHeight, -nearD);

	}

	return viewVec;

}







DecoScene::~DecoScene()
{
	tbb::mutex::scoped_lock lock(msMutex);
	DestroyAll();
	if (solidElements)
	{	
		INT xRes = 0, yRes = 0, zRes = 0;
#ifdef _DENSITY		
		DensityField::GetSingleton()->GetEffectResolution(xRes, yRes, zRes);
#endif
		if (xRes > numXElementVoxelize)
			Free3DArray(solidElements, xRes, yRes, zRes);
		else
			Free3DArray(solidElements, numXElementVoxelize, numYElementVoxelize, numZElementVoxelize);

		solidElements = NULL;
	}

	if (tmpVoxelizerBufferX)
	{
		delete[] tmpVoxelizerBufferX;
		tmpVoxelizerBufferX = NULL;
	}
	if (buffer)
	{
		delete[] buffer;
		buffer = NULL;
	}
	if (camera)
	{
		delete camera;
		camera = NULL;
	}


	if (RI)

	{

		RI->Destroy();

		delete RI;

		RI = NULL;

	}

}


void DecoScene::Render(BOOL bViewModel)

{

	//clear is done at caller, chenxz
	//RI->Clear();
	if (bViewModel)
   	    rootSceneNode->Render(RI, camera);
	DecoLight* Lights[MAX_LIGHT];
	INT numEffectiveLights = 0;
	rootSceneNode->GetRelevantLights(NULL, numEffectiveLights, Lights);
#ifdef _SLC
	SLC::GetSingleton()->Render(RI, Lights, numEffectiveLights);
#else
	//PLS::GetSingleton()->Render(RI, Lights, numEffectiveLights);
#endif
#ifdef _PARTICLES
	particles.Render(RI, Lights, numEffectiveLights, RP_Vorticity);
#endif
	//mCollisionDetector.Render(RI, Lights, numEffectiveLights);
}



void DecoScene::DestroyAll()
{
	selectedObjs.clear();
	vector<DecoSceneObject* > & sceneObjs =  rootSceneNode->GetSceneObjects();
	for(vector<DecoSceneObject* >::iterator it = sceneObjs.begin();
		it != sceneObjs.end(); ++it)
		delete *it;
	sceneObjs.clear();
	if (rootSceneNode)
		delete(rootSceneNode);
}



BOOL DecoScene::EraseAllSceneObject()
{

	vector<DecoSceneObject* > & sceneObjs =  rootSceneNode->GetSceneObjects();
	for(vector<DecoSceneObject* >::iterator it = sceneObjs.begin();
		it != sceneObjs.end(); ++it)
		delete *it;
	sceneObjs.clear();

	return TRUE;

}



DecoSceneObject* DecoScene::SelectObj(FLOAT x, FLOAT y)const

{

	DOUBLE depth = -1.f;

	vector3 origin, dir;

	PointScreen2WorldRay(vector2(x, y), origin,  dir);
	

	DecoSceneObject* retPtr = NULL;

	vector3 intersectionPT;

	//ASSERT(NULL != rootSceneNode);

	for(vector<DecoSceneObject* >::const_iterator it = rootSceneNode->GetSceneObjects().begin(); 
		it != rootSceneNode->GetSceneObjects().end(); ++it)
	{
		DecoSceneObject* obj = *it;
		if(obj->IsRenderable() && obj->IsSelectable())
		{	
			DOUBLE curdep = obj->GetRenderData()->GetBoundingBox().RayCheck(origin, dir);

			if(curdep > 0)
			{
				if (obj->GetRenderData()->RayIntersection(origin, dir, intersectionPT, curdep))
				{
					if (curdep < depth || depth < 0)
					{
						depth = curdep;
						retPtr = *it;
					}
				}
			}
		}
	}
	return retPtr;


}


void DecoScene::DeselectAll ()

{

	selectedObjs.erase(selectedObjs.begin(), selectedObjs.end());

}



BOOL DecoScene::IsSomethingSelected() const

{

	return (selectedObjs.size() > 0);

}


DecoSceneObject* DecoScene::GetFocusObject()const

{
	if(selectedObjs.size() > 0)

		return *(selectedObjs.begin());

	else

		return NULL;

}

DecoSceneObject* DecoScene::GetFirstObject()const
{
	return rootSceneNode->GetFirstSceneObj();
}

void DecoScene::SetFocusObject(DecoSceneObject* obj)

{

	selectedObjs.clear();

	if(obj != NULL)

        selectedObjs.push_back(obj);

}

void DecoScene::Destroy(DecoSceneObject* obj)
{
	if (!obj)
		return;
	for( vector<DecoSceneObject* >::iterator it = rootSceneNode->GetSceneObjects().begin(); 
		it != rootSceneNode->GetSceneObjects().end(); ++it)
	{
		if(*it == obj)
		{
			rootSceneNode->GetSceneObjects().erase(it);
			break;
		}
	}
	DecoSceneObject* focusObj = GetFocusObject();
	if (focusObj == obj)
		DeselectAll();
	delete obj;
}

void DecoScene::AddSceneObject(DecoSceneObject* solid)
{
	assert(solid && rootSceneNode);
	rootSceneNode->GetSceneObjects().push_back(solid);
}


DecoSceneObject* DecoScene::GetSceneObj(const char* name)
{
	vector<DecoSceneObject*> objs = rootSceneNode->GetSceneObjects();
	INT objSize = static_cast<INT>(objs.size());
	for (INT i = 0; i < objSize; i++)
	{
		DecoSceneObject* obj = objs[i];
		if (!strcmp(name, obj->GetName().c_str()))
			return obj;
	}
	return NULL;
}

void DecoScene::GetAllSceneObjects( vector<DecoSceneObject*> &allObjs)
{
	allObjs.clear();
	vector<DecoSceneObject*> objs = rootSceneNode->GetSceneObjects();
	allObjs.insert(allObjs.begin(), objs.begin(), objs.end());
}
void DecoScene::GetAllFreers ( vector<DecoSceneObject*> &allFreeers )
{
	allFreeers.clear();
	vector<DecoSceneObject*> objs = rootSceneNode->GetSceneObjects();
	INT objSize = static_cast<INT>(objs.size());
	for ( INT i = 0; i < objSize; i++ ){
		DecoSceneObject* obj = objs[i];
		if ( obj->GetType() == FreerT ){
			allFreeers.push_back( obj );
		}
	}
}

void DecoScene::PrepareSolidJacobian()
{
	int counter = 0;
	for (int i = 0; i < grids.numXElement; ++i)
	{
		for (int j = 0; j < grids.numYElement; ++j)
		{
			for (int k = 0; k < grids.numZElement; ++k)
			{
				if (grids.inDomain[i][j][k])
					counter++;
			}
		}

	}


	vector <DecoSceneObject*> allObjs;
	GetAllSceneObjects(allObjs);
	for (vector<DecoSceneObject*>::iterator it = allObjs.begin(); it != allObjs.end(); ++it)
	{
		if ((*it)->GetType() == SolidT || (*it)->GetType() == ArticulatedT)
		{
			DecoSolidObject* solidObj = static_cast<DecoSolidObject*>(*it);
			solidObj->PrepareImplicitTwowayCouplingMatrix(counter);
		}
	}

}

double DecoScene::OptimizeArticulatedQDot(DOUBLE seconds)
{
	vector <DecoSceneObject*> allObjs;
	GetAllSceneObjects(allObjs);
	for (vector<DecoSceneObject*>::iterator it = allObjs.begin(); it != allObjs.end(); ++it)
	{
		if ((*it)->GetType() == ArticulatedT)
		{
			
			DecoArticulatedObject* articulatedObj = static_cast<DecoArticulatedObject*>(*it);
			if (articulatedObj->mMode == TM_Planning)
			{
				SimpleStrokeOptimizer optimizer(this);
				optimizer.SetDesiredDirection(vector3(-1, 0, 0));
				optimizer.SetFluidField(&grids);
				optimizer.SetMatrices(solver.GetCSRMatrix(), solver.GetFluidMatrix());
				std::vector<double> weights;
				for (int i = 0; i < 12; ++i)
				{
					if (i < 6)
						weights.push_back(0.0);
					else
						weights.push_back(0.2);
				}
				optimizer.SetWeights(weights);
				std::vector<double> qdot;
				double value = optimizer.GetOptimalQDot(seconds, qdot);

				articulatedObj->SetQDot(qdot);
				return value;
			}
		}
	}
	return 1e30;
}


void DecoScene::Update(DOUBLE seconds)
{
	DecoArticulatedObject* articulatedObj = GetArticulatedObject();
	if (articulatedObj)
	{
		vector<double> q;
		articulatedObj->GetQ(q);
		Box prevScnBox = scnBox;
		grids.MoveGrid(vector3(q[0], q[1], q[2]) - scnBox.GetCenter(), &particles, this);
	}

	if(time == 0.0)
		UpdateSolidTransformation(true);
	else
		UpdateSolidBeforeCoupling();
	UpdateSolidTransformation();

#ifndef WATER_OFF
	VoxelizeSolid();
	solver.UpdateFluidGridType();

	PrepareSolidJacobian();

	solver.Solve();
	CalcForces();

	particles.Evolve();
#else

	vector<DecoSceneObject*> objs = rootSceneNode->GetSceneObjects();
	DecoSkelMesh* pMesh = (DecoSkelMesh*)objs[0]->GetRenderData();
	{
		DecoStaticMesh* pWing = (DecoStaticMesh*)pMesh->GetRenderDataFromIthBody(1);
		DecoStaticMeshSection* pSec = pWing->GetStaticMeshSection(0);
		DecoVertexBuffer* pVB = pSec->GetVertexBuffer();
		vector3 vert3 = pVB->GetVertices()[462];
		vector4 vert = vector4(vert3.x, vert3.y, vert3.z, 1);
		vert = pWing->GetLocalToWorld() * vert;
		mvTipTrace.push_back(vert);
	}
	{
		DecoStaticMesh* pWing = (DecoStaticMesh*)pMesh->GetRenderDataFromIthBody(3);
		DecoStaticMeshSection* pSec = pWing->GetStaticMeshSection(0);
		DecoVertexBuffer* pVB = pSec->GetVertexBuffer();
		vector3 vert3 = pVB->GetVertices()[360];
		vector4 vert = vector4(vert3.x, vert3.y, vert3.z, 1);
		vert = pWing->GetLocalToWorld() * vert;
		mvTipTraceBack.push_back(vert);
	}

#endif

	UpdateSolid();

	const SearcherInfo& info = DecoConfig::GetSingleton()->GetSearcherInfo();
	if (info.mbCollisionDetection)
		articulatedObj->CumulateCollisionVolume(CollisionDetection());
	time += seconds;
}
DOUBLE DecoScene::GetCurrentTime()
{
	return time;
}

void DecoScene::ResetCurrentTime()
{
	time = 0;
}

void DecoScene::Reset()
{
	ResetCurrentTime();
	
}

double DecoScene::CollisionDetection()
{
	mCollisionDetector.SetScene(this);
	double overlapVol = mCollisionDetector.CollisionDetection();
	(*DecoLogger::GetSingleton()) << "Colliding volume is " << overlapVol << ".\n";
	return overlapVol;
}

void DecoScene::DumpSolidGrid()
{
	grids.DumpSolid();
}

DecoArticulatedObject* DecoScene::GetArticulatedObject()
{
	vector <DecoSceneObject*> allObjs;
	GetAllSceneObjects(allObjs);
	for (vector<DecoSceneObject*>::iterator it = allObjs.begin(); it != allObjs.end(); ++it)
	{
		if ((*it)->GetType() == ArticulatedT)
			return static_cast<DecoArticulatedObject*>(*it);
	}
	return NULL;
}

void DecoScene::GatherAndOutputStatistics(int ithSample1, int numSample1, double p1, int ithSample2, int numSample2, double p2)
{
	static bool bFirstTime = true;
	DecoArticulatedObject* articulatedObj = GetArticulatedObject();
	std::vector<double> q;
	articulatedObj->GetQ(q);
	double value = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]);
	ofstream outFile, graphFile;
	if (bFirstTime)
	{
		outFile.open("..\\statistic.out", ios_base::trunc);
		graphFile.open("..\\graph.out", ios_base::trunc);
	}	
	else
	{
		outFile.open("..\\statistic.out", ios_base::app);
		graphFile.open("..\\graph.out", ios_base::app);

	}
	outFile << "( " << p1 << ", " << p2 << "):  " << value << std::endl;
	if (ithSample1 == numSample1 - 1)
	{
		graphFile << value << ";";

	}
	else
	{
		graphFile << value << " ";
	}

	outFile.close();
	graphFile.close();

	bFirstTime = false;
}

#ifdef _SLC
void DecoScene::TransformObjectToFluid (const string& objName)
{
	DecoSceneObject* obj = GetSceneObj(objName.c_str());
	const INT pathLen = 128;
	char filePath[pathLen];
	memset(filePath, 0, pathLen * sizeof(char));
	string modelPath;
	DecoConfig::GetSingleton()->GetString("Path", "modelPath", modelPath);
	sprintf(filePath, "%s%s.obj", modelPath.c_str(), objName.c_str());
	SLC::GetSingleton()->UnionMesh(filePath, obj->GetCoords().GetMatrix()); 
	grids.ClearTypeFlagTo(GT_Air);

	Destroy(obj);
}
#endif


void DecoScene::DumpSolidRib(const string& filename)
{
	vector<DecoSceneObject*> objs = rootSceneNode->GetSceneObjects();
	INT numObjs = static_cast<INT>(objs.size());
	for (INT i = 0; i < numObjs; i++)
	{
		DecoSceneObject* obj = objs[i];
		if (obj->GetType() != SolidT)
			continue;
		BOOL bDump = FALSE;
		DecoConfig::GetSingleton()->GetInt(obj->GetName().c_str(), "bDumpRib", bDump);
		if (bDump)
		{
			char fullName[256];
			memset(fullName, 0, 256 * sizeof(char));
			sprintf(fullName, "%s%s.rib", filename.c_str(), obj->GetName().c_str());
			(static_cast<DecoStaticMesh*>(obj->GetRenderData()))->DumpRibFile(fullName);
		}
	}
}

void DecoScene::Serialize(DecoArchive& Ar)
{
	Ar << (*rootSceneNode);
	Ar << scnBox << numXElement << numYElement << numZElement << stepSize;
	Ar << numXElementVoxelize << numYElementVoxelize << numZElementVoxelize;
	Ar << time;
	Ar << *(static_cast<DecoOpenGLRenderInterface*>(RI)) << (*camera);
	Ar << grids;
	solver.Serialize(Ar);
#ifdef _PARTICLES
	particles.Serialize(Ar);
#endif

#ifdef _SLC
	SLC::GetSingleton()->Serialize(Ar);
#endif
#ifdef _DENSITY
	DensityField::GetSingleton()->Serialize(Ar);
#endif


}
void DecoScene::Deserialize(DecoArchive& Ar)
{
	Ar >> (*rootSceneNode);
	Ar >> scnBox >> numXElement >> numYElement >> numZElement >> stepSize;
	Ar >> numXElementVoxelize >> numYElementVoxelize >> numZElementVoxelize;
	Ar >> time;
	Ar >> *(static_cast<DecoOpenGLRenderInterface*>(RI)) >> (*camera);

	Ar >> grids;
	DecoConfig::GetSingleton()->GetDouble("Scene", "timeStep", timeStep);
	AllocateTempBuffer();
	SetSceneBox(scnBox);
	SetGridsInformation();

	solver.Deserialize(Ar);
#ifdef _PARTICLES
	particles.Deserialize(Ar);
#endif

#ifdef _SLC
	SLC::GetSingleton()->Deserialize(Ar);
#endif
#ifdef _DENSITY
	DensityField::GetSingleton()->Deserialize(Ar);
#endif

}




DecoArchive& operator<< (DecoArchive& Ar, const DecoSceneNode& node)
{
	Ar << node.bIsLeaf;
	INT objNum = static_cast<INT>(node.sceneObjs.size());
	Ar << objNum;
	for (INT i = 0; i < objNum; i++)
	{
	    SceneObjectType type = node.sceneObjs[i]->GetType();
		Ar << static_cast<INT>(type);
		switch(type)
		{
		case FluidT:	
			Ar << *(static_cast<DecoFluidObject*>(node.sceneObjs[i]));
			break;
		case SolidT:
			Ar << *(static_cast<DecoRigidObject*>(node.sceneObjs[i]));
			break;
		case ArticulatedT:
			Ar << *(static_cast<DecoArticulatedObject*>(node.sceneObjs[i]));
			break;
		case SprayerT:
			Ar << *(static_cast<DecoSprayerObject*>(node.sceneObjs[i]));
			break;
		default:
			break;
		}
	}
	return Ar;
}
DecoArchive& operator>> (DecoArchive& Ar, DecoSceneNode& node)
{
	Ar >> node.bIsLeaf;
	UINT objNum = 0;
	Ar >> objNum;
	for (UINT i = 0; i < objNum; i++)
	{
		INT type = 0;
		Ar >> type;
		if (type == FluidT)
		{
			DecoFluidObject* obj = new DecoFluidObject();
			Ar >> (*obj);
			node.sceneObjs.push_back(obj);
		}
		else if (type == SolidT)
		{
			DecoRigidObject* obj = new DecoRigidObject(); 
			Ar >> (*obj);
			node.sceneObjs.push_back(obj);
		}
		else if (type == SprayerT)
		{
			DecoSprayerObject* obj = new DecoSprayerObject;
			Ar >> (*obj);
			node.sceneObjs.push_back(obj);
		}
		else if (type == ArticulatedT)
		{
			DecoArticulatedObject* obj = new DecoArticulatedObject;
			Ar >> (*obj);
			node.sceneObjs.push_back(obj);
		}
	}

	return Ar;
}

